#include "world.h"
#include "body.h"
#include "box.h"
#include "util.h"

World::World(void)
{
	bodyID = jointID = 0;
	maxWorldObjects = 100;
	maxParticles = 64000;
	for(int i = 0; i < maxWorldObjects; i++){
		bodies[i] = NULL;
		joints[i] = NULL;
	}
	for(int i = 0; i < maxParticles; i++)
		particles[i] = NULL;
	for(int i = 0; i < 16; i++)
		pEmitters[i] = NULL;
	numBodies = 0;
	numJoints = 0;
	numParticles = 0;
	numpEmitters = 0;

	floorVertices = new Vector4*[4];
	floorVertices[0]=new Vector4(-200.f, 0.f, 200.f);
	floorVertices[1]=new Vector4(-200.f, 0.f,-200.f);
	floorVertices[2]=new Vector4( 200.f, 0.f,-200.f);
	floorVertices[3]=new Vector4( 200.f, 0.f, 200.f);
}


void World::draw(Vector4 *camPos, Vector4 *camRot)
{
	//std::cout << "num particles: " << numParticles << std::endl;
	drawFloor();
	for (int i = 0; i < numBodies; i++)
		bodies[i]->draw(camPos, camRot);
	for (int i = 0; i < numJoints; i++)
		joints[i]->draw();
	for (int i = 0; i < numParticles; i++)
		particles[i]->draw(camPos, camRot);

	//std::cout << "floor Normal " << floorNormal[0] << " " << floorNormal[1] << " " << floorNormal[2] << std::endl;
}

void World::update(float etime)
{
	//std::cout << etime <<std::endl;
	
	for (int i = 0; i < numBodies; i++)
		bodies[i]->update(etime);
	for (int i = 0; i < numJoints; i++)
		joints[i]->update(etime);

	for (int i = 0; i < numParticles; i++){
		bool pUpdate = particles[i]->update(etime);
		Vector4 floorNormal = Util::getNormal(floorVertices[0], floorVertices[2], floorVertices[1]);
		Vector4 *intersection = new Vector4();
		bool colFloorPlane = Util::segmentPlaneCollision(&(particles[i]->pos), &(particles[i]->lastPos), floorVertices[0], &(floorNormal), intersection);
		if (colFloorPlane == true){
			float colFloor = Util::CalcAngleSum(intersection, floorVertices, 4);
			if (colFloor >= 6.0f && colFloor <= 6.6f){
				particles[i]->destroy = true;
			}
			else{
				std::cout << "colFloor - " << colFloor << std::endl;
			}
		}
		else if(checkParticleAgainstBodies(particles[i])){
			//std::cout << "Holy crap I hit a cube!" <<std::endl;
			particles[i]->destroy = true;
		}
	

		if(pUpdate == false || particles[i]->destroy == true){
			//std::cout << "destroyed particle!" <<std::endl;
			free(particles[i]);
			for(int j=i; j<numParticles-1; j++)
				particles[j] = particles[j+1];
			numParticles -= 1;
		}
	}
	for (int i = 0; i < numpEmitters; i++)
	{
		int num = 0;
		Particle **newParticles = pEmitters[i]->update(etime, num);
		if (numParticles + num > maxParticles){
			std::cout << "TOO MANY PARTICLES" << std::endl;
			return;
		}
		for(int i=0; i<num; i++){
			particles[numParticles+i] = newParticles[i];
		}
		numParticles += num;
	}
}

bool World::checkParticleAgainstBodies(Particle *p){
	for (int i = 0; i < numBodies; i++){
		bool isHit = Util::doesParticleIntersectBox(p, bodies[i]);
		if (isHit == true){
			Vector4 forceToApply = p->acc;
			forceToApply.scale(-9000.0f);
			bodies[i]->applyForce(Force(p->pos[0],p->pos[1],p->pos[2],forceToApply));
			Vector4 impulseToApply = p->vel;
			impulseToApply.scale(1.0f/1000.f);
			bodies[i]->impulse = bodies[i]->impulse + impulseToApply;
			return true;
		}
	}
	return false;

}
void World::addParticleEmitter(ParticleFactory *pf)
{
	std::cout << "Adding particle emitter" << std::endl;
	pEmitters[numParticles] = pf;
	numpEmitters++;
	std::cout << "done" << std::endl;
}

int World::addBody(Body *newBody){
	if (bodyID >= maxWorldObjects){
		std::cout << "Can't add another body!" << std::endl;
		return -1;
	}
	if (bodies[bodyID] != NULL){
		std::cout << "There is already something in this body ID spot!" << std::endl;
		return -1;
	}
	bodies[bodyID] = newBody;
	bodies[bodyID]->id = bodyID;
	std::cout<< "I added a body with bodyID: " << bodyID << std::endl;
	numBodies++;
	return bodyID++;
}

int World::addJoint(Joint *newJoint){
	if (jointID >= maxWorldObjects){
		std::cout << "Can't add another joint!" << std::endl;
		return -1;
	}

	if (joints[jointID] != NULL){
		std::cout << "There is already something in this joint ID spot!" << std::endl;
		return -1;
	}
	joints[jointID] = newJoint;
	std::cout<< "I added a joint with jointID: " << jointID;
	jointID++;
	return jointID++;

}

void World::drawFloor()          // Draws The Floor
{
//glBindTexture(GL_TEXTURE_2D, floorTexture[0]);   // Select Texture 1 (0)
 glBegin(GL_QUADS);         // Begin Drawing A Quad
  glNormal3f(0.0, 1.0, 0.0);      // Normal Pointing Up
  glColor3f(.2f, .2f, .2f);
   //glTexCoord2f(0.0f, -1.0f);     // Bottom Left Of Texture
   glVertex3f(floorVertices[0]->get(0),floorVertices[0]->get(1),floorVertices[0]->get(2));     // Bottom Left Corner Of Floor
   
   //glTexCoord2f(0.0f, 0.0f);     // Top Left Of Texture
   glVertex3f(floorVertices[1]->get(0),floorVertices[1]->get(1),floorVertices[1]->get(2));     // Top Left Corner Of Floor
   
   //glTexCoord2f(1.0f, 0.0f);     // Top Right Of Texture
   glVertex3f(floorVertices[2]->get(0),floorVertices[2]->get(1),floorVertices[2]->get(2));     // Top Right Corner Of Floor
   
   //glTexCoord2f(1.0f, 1.0f);     // Bottom Right Of Texture
   glVertex3f(floorVertices[3]->get(0),floorVertices[3]->get(1),floorVertices[3]->get(2));     // Bottom Right Corner Of Floor
 glEnd();           // Done Drawing The Quad
}

